#include <stdio.h>
#include <stdlib.h>
#include <assert.h>
#include <string.h>
#include <float.h>

#define LEFT 0
#define RIGHT 1
#define DISTANCE 2

/**
 * Compare two nodes by distance. Used for qsort().
 */
static int nodecompare(const void *a, const void *b)
{
	const float *node1 = (const float *)a;
	const float *node2 = (const float *)b;
	const float term1 = node1[DISTANCE];
	const float term2 = node2[DISTANCE];
	if (term1 < term2)
		return -1;
	if (term1 > term2)
		return +1;
	return 0;
}

/**
 * Calculates the sum of [1..n].
 */
static __inline int sumn(int n)
{
	return n*(n+1)/2;
}

/**
 * Computes the single linkage of the input pairwise distances.
 *
 * result should be allocated to sizeof(float)*size_y*3.
 */
extern "C" __declspec(dllexport) void
run_linkage(float *result, float *input, const unsigned int size_y)
{
	int i, j, k;
	float *temp;
	int *index, *vector;
	int nelements = size_y;
	int nnodes = size_y - 1;

	temp = (float *)malloc(sizeof(float) * (int)(nelements*(nelements+1)/2));
	//assert(temp);
	vector = (int *)malloc(sizeof(int) * nelements*(nelements+1)/2);
	//assert(vector);
	index = (int *)malloc(sizeof(int) * (nelements*(nelements+1)/2 - 1));
	//assert(index);

	for (i = 0; i < nelements; i++)
	{
		vector[i] = i;
		result[i*3+LEFT] = (float)i;
		result[i*3+DISTANCE] = (float)FLT_MAX;
		index[i] = i;
	}

	for (i = 0; i < nelements; i++)
	{
		// find the corresponding row vector
		for (j = 0; j < i; j++)
		{
			// formula to convert (i,j) into the corresponding input row
			// vector coordinate:
			// (i-1) + sum_1^(nnodes-1) n + sum_1^(nnodes-1-j) n
			temp[j] = input[i-1 + sumn(nnodes-1) - sumn(nnodes-1-j)];
		}

		for (j = 0; j < i; j++)
		{
			k = vector[j];
			if (result[j*3+DISTANCE] >= temp[j])
			{
				if (result[j*3+DISTANCE] < temp[k])
					temp[k] = result[j*3+DISTANCE];
				result[j*3+DISTANCE] = temp[j];
				vector[j] = i;
			}
			else if (temp[j] < temp[k])
			{
				temp[k] = temp[j];
			}
		}

		for (j = 0; j < i; j++)
		{
			if (result[j*3+DISTANCE] >= result[vector[j]*3+DISTANCE])
				vector[j] = i;
		}
	}

	// sort the nodes based on distance
	qsort(result, nelements, sizeof(float)*3, nodecompare);

	for (i = 0; i < nnodes; i++)
	{
		j = (int)result[i*3+LEFT];
		k = vector[j];
		if (index[j] < index[k])
		{
			result[i*3+LEFT] = (float)index[j]+1;
			result[i*3+RIGHT] = (float)index[k]+1;
		}
		else
		{
			result[i*3+RIGHT] = (float)index[j]+1;
			result[i*3+LEFT] = (float)index[k]+1;
		}
		index[k] = nnodes+i+1;
	}

	free(index);
	free(vector);
	free(temp);
}

